//========================================================================
// FILENAME : MainProgram.c
// CREATED : 07-03-2009
// AUTHOR : Jesper Aagaard Vuholm
// DESCR. : This file is the main / test file of the DIYS project.
//------------------------------------------------------------------------
//
// REV. DATE/AUTHOR CHANGE DESCRIPTION
// 1.00 07-02-2009/Jesper Delay functions tested.
// 1.01 11-02-2009/Jesper UART functions tested.
// 1.02 12-03-2009/Jesper Driver for right motor tested.
// 1.03 15-03-2009/Jesper Driver for left motor tested.
// 1.04 16-03-2009/Jesper ADC tested, streaming resuslts to c++ program.
// 1.05 18-04-2009/Jesper Angle calculations tested, streaming results
//                        to bray terminal, results are capture and used
//                        for later analysis.
// 1.06 05-05-2009/Jesper Control loop tested with looptimer and is working.
// 1.07 05-05-2009/Jesper A remote pc/mobile phone can now change the KP KD
//		             	  and KS values, these control the "responsiveness" of
//                        our Segway.
// 1.08 07-05-2009/Jesper Radio ON / OFF control added, now the radio can be
//                        turned on/off from a remote computer/mobile phone.
//========================================================================
#include <avr/io.h>
#include <stdlib.h>
#include <avr/interrupt.h>

#include "Drivers/looptimer/LoopTimerDriver.h"
#include "util/Delay.h"
#include "Drivers/adc/AdcDriver.h"
#include "Drivers/motor/MotorDriver.h"
#include "Drivers/uart/UartDriver.h"

#define sbi(port,pin)    port |= 1<<pin    //set bit
#define cbi(port,pin)    port &= ~(1<<pin) //clear bit

// setup defines
#define UART_BAUD_RATE 		9600
#define F_CPU				10000000
#define STREAM_BUFFER_SIZE 	32
#define INIT_CNT 			3000
#define STREAM_CNT_MAX 		50


// angle math defines & variables
#define OFFSET_ADC			512
#define OFFSET_ADC_JOYSTICK 548
#define ADC_PR_DEG_PR_SEC 	4.1

double angle = 0.0;
double acc_deg = 0.0;
double deg_per_sec = 0.0;
double gyro_deg = 0.0;
double avgAcc = 0.0;

// motor/pwm related values
signed int PWM = 0;
signed int joystick = 0;

// timer 0 Setup
unsigned int loop_time = 0;
unsigned int dt = 0;

// stream values
unsigned char STREAM = 0; //0 to disable
double angle_to_stream = 0.0;
double acc_to_stream = 0.0;
double gyro_to_stream = 0.0;

// PD controlling and steering values
double KP = 48; 	// proportional
double KD = 1.5;  	// derivative
double KS = 1.8;	// steering factor

// test functions prototypes.
void transive_bluetooth_data(void);
void stream_KP_KD_KS_PWM(void);
void streamJoystickRaw(void);
void streamPWM(void);
void streamData(void);
void streamAccGyroRaw(void);
void streamAccGyroAvg(void);
void streamAccAndGyroRawAndPWNAndDT(void);
void streamThe3Angels(void);
void streamAccAndGyroRawAndAngleAndDT(void);

// timer 0 overflow vector used in main loop generating DT.
ISR(TIMER0_OVF_vect)
{
	//reloads the timer0 counter from 131 = 100microsec per roll-over.
	TCNT0 = 131;
	loop_time++;
}


int main(void)
{
	init_System();
	unsigned int stream_cnt = 0;

	while(1)
	{
		dt = loop_time;
		loop_time = 0;

		//Joystick calcs
		joystick = ((OFFSET_ADC_JOYSTICK - (signed int)getJoystick()) >> 3);
		joystick *= KS;

		//Acc calcs
		avgAcc = ((avgAcc * 4) + getAcclerometer()) / 5;
		acc_deg =	((avgAcc - (double)OFFSET_ADC) * (4/3));

		//Gyro calcs
		deg_per_sec = (((double)OFFSET_ADC - (double)getGyro())/ADC_PR_DEG_PR_SEC);
		gyro_deg += deg_per_sec * (double)dt * 0.0001;

		//Angle calcs(aproximation)
		angle = 0.994 * (angle + deg_per_sec * (double)dt * 0.0001);
		angle += 0.006 * (acc_deg);

		// PD->PWM calcs
		PWM = (((KP * angle) + (KD * (double)deg_per_sec)));

		//Pulse the motors
		runMotors(PWM , PWM, joystick);


		if(stream_cnt > STREAM_CNT_MAX)
		{
			// stream specific, out/in comment the function to start the desired stream.
			acc_to_stream = acc_deg * 100;
			gyro_to_stream = deg_per_sec* 100;
			angle_to_stream = angle* 100;

			transive_bluetooth_data(); // default stream.
			//		streamAccGyroAvg();
			//		streamAccAndGyroRawAndAngleAndDT();
			//		streamAccAndGyroRawAndAngleAndPWNAndDT();
			//		streamJoystickRaw();
			//		streamAccGyroRaw();
			//		streamDT();
			//		stream_KP_KD_KS_PWM();
			stream_cnt = 0;
		}
		else
		{
			stream_cnt++;
		}
	}
}

void init_System(void)
{
	// Enable UART
	uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) );
	sei();

	// Enable ADC
	ADC_init();

	// Enable motors
	motor_init();

	// Enable looptimer
	looptimer_init();

	angle = 0;
	unsigned int i = 0;
	loop_time = 0;
	while(i < INIT_CNT)
	{
		dt = loop_time;
		loop_time = 0;

		//Acc calcs
		avgAcc = ((avgAcc * 4) + getAcclerometer()) / 5;
		acc_deg =	((avgAcc - (double)OFFSET_ADC) * (4/3));

		i++;
	}
	angle = acc_deg;
	BEEP(50);
}


void streamAccGyroRaw(void)
{
	char end_of_data[1];
	end_of_data[0]='$';

	char seperator[1];
	seperator[0]=',';

	char end_of_transmission[1];
	end_of_transmission[0]='\n';

	char data_stream_buffer[STREAM_BUFFER_SIZE];
	memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

	char tmp_val[5];

	itoa(ADC_raw_values[ACC], tmp_val, 10);
	strncat(data_stream_buffer, tmp_val, 5);
	strncat(data_stream_buffer, seperator, 1);

	itoa(ADC_raw_values[GYRO], tmp_val, 10);
	strncat(data_stream_buffer, tmp_val, 4);

	strncat(data_stream_buffer, "\n", 1);

	uart_puts(data_stream_buffer);

	memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
}


void stream_KP_KD_KS_PWM(void)
{
	char seperator[1];
	seperator[0]=',';

	char data_stream_buffer[STREAM_BUFFER_SIZE];
	memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE-1);

	char tmp_val[7];

	//KP
	itoa((KP), tmp_val, 10);
	strncat(data_stream_buffer, tmp_val, 7);
	strncat(data_stream_buffer, seperator, 1);

	//KD
	itoa((KD*10), tmp_val, 10);
	strncat(data_stream_buffer, tmp_val, 7);
	strncat(data_stream_buffer, seperator, 1);

	//KS
	itoa((KS*10), tmp_val, 10);
	strncat(data_stream_buffer, tmp_val, 4);
	strncat(data_stream_buffer, seperator, 1);

	//PWM
	itoa(PWM, tmp_val, 10);
	strncat(data_stream_buffer, tmp_val, 4);
	strncat(data_stream_buffer, "\n", 1);

	uart_puts(data_stream_buffer);
	memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
}

void transive_bluetooth_data(void)
{
	unsigned int c;
	c = uart_getc();

	if (c & UART_NO_DATA)
	{
		if(STREAM)
		{
			char end_of_data[1];
			end_of_data[0]='$';

			char seperator[1];
			seperator[0]=',';

			char end_of_transmission[1];
			end_of_transmission[0]='\n';

			char data_stream_buffer[STREAM_BUFFER_SIZE];
			memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

			char tmp_val[7];

			//Acc raw 1
			itoa(acc_to_stream, tmp_val, 10);
			strncat(data_stream_buffer, tmp_val, 5);
			strncat(data_stream_buffer, seperator, 1);

			//Gyro raw 2
			itoa(gyro_to_stream, tmp_val, 10);
			strncat(data_stream_buffer, tmp_val, 5);
			strncat(data_stream_buffer, seperator, 1);

			//Angle stream 3
			itoa(angle_to_stream, tmp_val, 10);
			strncat(data_stream_buffer, tmp_val, 7);
			strncat(data_stream_buffer, seperator, 1);

			//PWM 4
			itoa(PWM, tmp_val, 10);
			strncat(data_stream_buffer, tmp_val, 7);
			strncat(data_stream_buffer, seperator, 1);

			//Joystick 5
			itoa(joystick, tmp_val, 10);
			strncat(data_stream_buffer, tmp_val, 4);
			strncat(data_stream_buffer, end_of_data, 1);

			//crc
			itoa(create_checksum(data_stream_buffer), tmp_val, 10);
			strncat(data_stream_buffer, tmp_val, 5);

			strncat(data_stream_buffer,end_of_transmission,1);
			uart_puts(data_stream_buffer);

			memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
		}
	}
	else
	{
		if ( c & UART_FRAME_ERROR )
		{
			/* Framing Error detected, i.e no stop bit detected */
			uart_puts("UART Frame Error: ");
		}
		if ( c & UART_OVERRUN_ERROR )
		{
			/*
			 * Overrun, a character already present in the UART UDR register was
			 * not read by the interrupt handler before the next character arrived,
			 * one or more received characters have been dropped
			 */
			uart_puts("UART Overrun Error: ");
		}
		if ( c & UART_BUFFER_OVERFLOW )
		{
			/*
			 * We are not reading the receive buffer fast enough,
			 * one or more received character have been dropped
			 */
			uart_puts("Buffer overflow error: ");
		}
		/*
		 * Handle the incomming request
		 */

		//uart_putc( (unsigned char)c );

		char UART_In_Buffer[32];
		memset(UART_In_Buffer, '\0', 32-1);

		// 1=P, 2=D 3
		//unsigned char inputType = 0;

		// delete me after testing
		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
		// delete me after testing

		char tmp_val[7];
		char tmp_valTal[7];
		char lastrcv ='|';
		unsigned char cnt = 0;
		double tmp = 0;

		if(c != 0)
		{
			switch(c)
			{
			case 'P':
				cnt = 0;
				while(lastrcv != '~')
				{
					lastrcv = uart_getc();
					if(is_a_number(lastrcv))
					{
						tmp_val[cnt] = lastrcv;
						cnt++;
					}
				}
				tmp = atoi(tmp_val);
				tmp /= 10;
				KP = tmp;
				tmp = 0;
				BEEP(20);
				memset(tmp_val,'\0',6);
				itoa(KP, tmp_valTal, 10);
				break;
			case 'D':
				cnt = 0;
				while(lastrcv != '~')
				{
					lastrcv = uart_getc();
					if(is_a_number(lastrcv))
					{
						tmp_val[cnt] = lastrcv;
						cnt++;
					}
				}
				tmp = atoi(tmp_val);
				tmp /= 10;
				KD = tmp;
				tmp = 0;
				BEEP(20);
				memset(tmp_val,'\0',6);
				itoa(KD, tmp_valTal, 10);
				break;
			case 'S':
				cnt = 0;
				while(lastrcv != '~')
				{
					lastrcv = uart_getc();
					if(is_a_number(lastrcv))
					{
						tmp_val[cnt] = lastrcv;
						cnt++;
					}
				}
				tmp = atoi(tmp_val);
				tmp /= 10;
				KS = tmp;
				tmp = 0;
				BEEP(20);
				memset(tmp_val,'\0',6);
				break;
			case 'T':
				while(lastrcv != '~')
				{
					lastrcv = uart_getc();

				}
				if(STREAM)
					STREAM = 0;
				else
					STREAM = 1;
				BEEP(20);
				break;
			default:
				// Something went wrong if we are here.
				uart_putc(c);
				c = uart_getc();
				BEEP(100);
				break;
			}
		}
	}

	void streamJoystickRaw(void)
	{
		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[7];

		//Joystick
		itoa(joystick, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 4);
		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);
		memset(tmp_val,'\0',8-1);
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
	}

	void streamAccAndGyroRawAndAngleAndPWNAndDT(void)
	{
		char end_of_data[1];
		end_of_data[0]='$';

		char seperator[1];
		seperator[0]=',';

		char end_of_transmission[1];
		end_of_transmission[0]='\n';

		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[7];

		//Acc raw 1
		itoa(acc_deg, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 5);
		strncat(data_stream_buffer, seperator, 1);

		//Gyro raw 2
		itoa(deg_per_sec, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 5);
		strncat(data_stream_buffer, seperator, 1);

		//Angle stream 3
		itoa(angle_to_stream, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 7);
		strncat(data_stream_buffer, seperator, 1);

		//PWM 4
		itoa(PWM, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 7);
		strncat(data_stream_buffer, seperator, 1);

		//DT 5
		itoa(dt, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 4);
		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
	}

	void streamAccAndGyroRawAndAngleAndDT(void)
	{
		char end_of_data[1];
		end_of_data[0]='$';

		char seperator[1];
		seperator[0]=',';

		char end_of_transmission[1];
		end_of_transmission[0]='\n';

		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[6];

		itoa(ADC_raw_values[ACC], tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 6);
		memset(tmp_val,'\0',6);

		strncat(data_stream_buffer, seperator, 1);

		itoa(ADC_raw_values[GYRO], tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 6);
		memset(tmp_val,'\0',6);

		strncat(data_stream_buffer, seperator, 1);

		itoa((int)(angle_to_stream), tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 6);
		memset(tmp_val,'\0',6);

		strncat(data_stream_buffer, seperator, 1);

		itoa(dt, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 4);

		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);

		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

	}

	void streamThe3Angels(void)
	{
		char end_of_data[1];
		end_of_data[0]='$';

		char seperator[1];
		seperator[0]=',';

		char end_of_transmission[1];
		end_of_transmission[0]='\n';

		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[8];

		strncat(data_stream_buffer, "A", 1);

		itoa((int)(acc_deg*100), tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 8);

		memset(tmp_val,'\0',8-1);

		strncat(data_stream_buffer, "B", 1);

		itoa((int)(gyro_deg*100), tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 8);

		memset(tmp_val,'\0',8-1);

		strncat(data_stream_buffer, "C", 1);

		itoa((int)(angle*100), tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 8);
		memset(tmp_val,'\0',8-1);
		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);

		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
	}

	void streamDT(void)
	{

		char end_of_transmission[1];
		end_of_transmission[0]='\n';

		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[5];

		itoa(dt, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 4);


		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);

		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

	}

	void streamAngle(void)
	{

		char end_of_transmission[1];
		end_of_transmission[0]='\n';

		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[5];

		//itoa((int)(angle), tmp_val, 10);
		itoa((int)(angle_to_stream),tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 5);

		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);

		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

	}


	void streamAccGyroAvg(void)
	{
		char end_of_data[1];
		end_of_data[0]='$';

		char seperator[1];
		seperator[0]=',';

		char end_of_transmission[1];
		end_of_transmission[0]='\n';

		char data_stream_buffer[STREAM_BUFFER_SIZE];
		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);

		char tmp_val[5];


		itoa(avgAcc, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 5);

		strncat(data_stream_buffer, seperator, 1);


		itoa(dt, tmp_val, 10);
		strncat(data_stream_buffer, tmp_val, 4);

		strncat(data_stream_buffer, "\n", 1);

		uart_puts(data_stream_buffer);

		memset(data_stream_buffer, '\0', STREAM_BUFFER_SIZE);
	}
}
